/*
 * TestBench.c
 *
 * Created: 4/5/2013 9:43:10 AM
 *  Author: Parag
 */ 

#ifndef	F_CPU
#define F_CPU 8000000
#endif

#include <avr/io.h>
#include <util/delay.h>
#include <stdio.h>
#include <stdbool.h>
#include "U0_UART.h"
#include "Servo.h"
#include "Motor_Driver.h"

#define MaxDepth				120					//7 psi
#define MinDepthBuffer			25					// since we want the min trigger to hit early then use this to add on the initial reading onto initial surface reading
#define MotorSeconds			4
#define NRcycles				10					// everytime the SWP surfaces; it completes 1 cycle

void System(int P);

extern FILE uart_stream;

int i,Pressure,pumpFrontTime,pumpBackTime;
char  PString[4];
_Bool PressureMaxFlag, PressureMinFlag;

pumpFrontTime = 21;// (seconds) since we are putting it through a minute each loop
pumpBackTime = 23;
PressureMinFlag = true;		//default conditions
PressureMaxFlag = false;	

int main(void)
{
	stderr = stdout = stdin = &uart_stream;	
	UARTInit();						/* Redirecting the standard I/O */
	ServoInit();
	MotorInit();

	
//	Motor_Adjust();
	
	printf("Initialize Pressure:");	
	if(fgets(PString,5,stdin) != NULL)
	{
		Pressure = atoi(PString);
		MinDepth = Pressure + MinDepthBuffer;
		printf("\n");
	}

	
		while(1)
		{
			printf("Set Pressure:");
			if(fgets(PString,5,stdin) != NULL)
			{
				Pressure = atoi(PString);
				printf("\n");
			}

			System(Pressure);		
		}	
	


	return 1;
}	


void System(int P)
{
	
	static int TotalCycles = 0;
	/*
		future code
		if TotalCycles is blah set delay time to change direction(turn it around)
		if Total Cycles is blah set the delay time and change rudder position
		'	'		'		'		''		'	'	'	'	'	'	'	'	'
		*/

	// o no! i have reached minDepth
	if((P < MinDepth) && (PressureMinFlag == true))
	{
		printf("// o no! i have reached minDepth\n");		
			
		if(TotalCycles != NRcycles)
		{
			PressureMinFlag = false;					// this will flag that correction has been applied
			MotorOnBack();
			for(i = 0; i <pumpFrontTime;i++)
			{
				_delay_ms(1000);						//1 second
			}	
		}// if TotalCycles	is not reached		
		//if you have reached the NR of cycles specified then just float up
		//else do absolutely nothing and let it float up
	}
	// o no! max Depth reached
	else if((P > MaxDepth) && (PressureMaxFlag == false))
	{
		printf("// o no! max Depth reached\n");	
		PressureMaxFlag = true;							// this will flag that the correction has been applied
		MotorOnForward();
		for(i = 0; i <pumpBackTime;i++)
		{
			_delay_ms(1000);							//1 second
		}
		
	}	
	// im still at max depth! WTF
	else if((P> MaxDepth) && (PressureMaxFlag == true))
	{
		//chill out and do nothing/ wait for the correction to materialize
	}
	// finally im rising! correction is working
	else if((P < MaxDepth) && (PressureMaxFlag == true))
	{
		printf("// finally im rising! correction is working\n");
		PressureMaxFlag = false;						//restore// this will flag that the correction worked
		TotalCycles++;									///////////a cycle is complete when it starts to actually rise//////////////////////////////
	}	
	// im still at min Depth! WTF
	else if((P < MinDepth) && (PressureMinFlag == false))
	{
		// chill out and enjoy the inevitable dive//  wait for correction to materialize
	}
	// finally im diving!
	else if((P > MinDepth) && (PressureMinFlag == false))
	{
		printf("// finally im diving! correction is working\n");
		PressureMinFlag = true;						//restore// this will flag that the correction worked
	}
	else
	{
		// no op/ enjoy the ride/ nothing the worry about
	}		
}	